//此文件为Transducer——M IMU陀螺仪驱动，由电控组王川负责  电话 17609531594
#include "device.h"
#include "string.h"
#include "module.h"
float v_x = 0;
float v_z = 0;
float v_y = 0;
float a_x = 0;
float a_z = 0;
float a_y = 0;
float a_z_last = 0;
int16_t circle_yaw = 0;
uint8_t mpu_flag = 0;
uint8_t first_flag = 0;
uint16_t count_23 = 0,count_47 = 0;
static float Trans_Char2Float(uint8_t *pData);
static uint16_t Checksum_Generate(char *data, int dataLength);

/******************************************************
 * @data	2018.11.04 23:05
 * @brie	TransducerIMU初始化
 * @input	_P_IMU_HANDLE   IMU的句柄
 * @output	无
 * @mader	王川	17609531594
 * ***************************************************/
 void Init_IMU(P_IMU_HANDLE pimu_hadl)
 {
    pimu_hadl->state.packstate = IMU_PACK_STATE_HEADER;
    UART5_Init(1000000);
 }

 /******************************************************
 * @data	2018.11.04 23:16
 * @brie	TransducerIMU数据迭代
 * @input	_P_IMU_HANDLE   IMU的句柄  data:要迭代的数据
 * @output	无
 * @mader	王川	17609531594
 * ***************************************************/
 void TransducerM_Rev(P_IMU_HANDLE imu,uint8_t data)
 {
    static uint8_t head_cout = 0;
    static uint8_t data_count = 0;
    static uint8_t CRC_check[2] = {0};

	uint8_t byte[4] = {0};
     switch(imu->state.packstate)
     {
         case IMU_PACK_STATE_HEADER:
            if(head_cout == 0)
            {
                if(data == TRANSDUCER_HEAD_1)
                {
                    head_cout = 1;
                    data_count = 0;
                }
            }
            else 
            {
                if(data == TRANSDUCER_HEAD_2)
                {
                    imu->state.packstate = IMU_PACK_STATE_LENTH;
                }
                head_cout = 0;
            }
            break;

        case IMU_PACK_STATE_LENTH :
						if(data == 0x14)
						{
							imu->state.length=23;
							imu->state.data[data_count++] = data;
							imu->state.packstate = IMU_PACK_STATE_DATA;
						}
						else if(data == 0x2C)
						{
							imu->state.length=47;
							imu->state.data[data_count++] = data;
							imu->state.packstate = IMU_PACK_STATE_DATA;							
						}
            else imu->state.packstate = IMU_PACK_STATE_HEADER;
            break;

        case IMU_PACK_STATE_DATA:
            imu->state.data[data_count] = data;
            data_count++;
            if(data_count > 50)
            {
                data_count = 0;
                head_cout = 0;
                imu->state.packstate = IMU_PACK_STATE_HEADER;
            }
            if(data_count == imu->state.length)
            {
                imu->state.packstate = IMU_PACK_STATE_HEADER;
                int i,j;
                unsigned char*d = (unsigned char*)imu->state.data;
                unsigned char c;
                uint16_t imu1500_check = 0xffff;
                for(i=0; i<imu->state.length-2; i++)
                {
                    imu1500_check ^= (unsigned int)(*(d++));
                    for(j=0; j<8; j++)
                    {
                        c  = imu1500_check & 0x0001;
                        imu1500_check >>= 1;
                        if(c) imu1500_check ^= 0xa001;
                    }
                }
                memcpy(CRC_check, &imu1500_check, 2 );
            }
            if(CRC_check[0]==imu->state.data[imu->state.length-2]&&CRC_check[1]==imu->state.data[imu->state.length-1])
			{			
				if(imu->state.length==23)
				{
					
          byte[0] = imu->state.data[13];
					byte[1] = imu->state.data[14];
					byte[2] = imu->state.data[15];
					byte[3] = imu->state.data[16];	
					memcpy(&a_y,byte,sizeof(float));
					
					       imu->data.angle.roll = a_y;				
					byte[0] = imu->state.data[17];
					byte[1] = imu->state.data[18];
					byte[2] = imu->state.data[19];
					byte[3] = imu->state.data[20];	
					memcpy(&a_x,byte,sizeof(float));
//                  if(a_x > 200) a_x-= 360; 
                    imu->data.angle.pitch = a_x;
					
          byte[0] = imu->state.data[9];
					byte[1] = imu->state.data[10];
					byte[2] = imu->state.data[11];
					byte[3] = imu->state.data[12];
                    a_z_last = 	a_z;
					memcpy(&a_z,byte,sizeof(float));
                    if(imu->data.angle.first_flag == 0)
                    {
						if(a_z != 0)
						{
                          imu->data.angle.first_yaw = a_z;
						  if((a_z -  imu->data.angle.first_yaw)  > 10 ||(a_z -  imu->data.angle.first_yaw)  < (-10 ))
							{
								imu->data.angle.first_flag = 0;
							}
						  else
                           {
                                imu->data.angle.first_flag = 1;
                            }
						}
                    }
                    else if(imu->data.angle.first_flag == 1)
                    {
                        if(a_z - a_z_last > 280)//从0度到360度突变
                        {
                            circle_yaw--;
                        }
                        else if(a_z - a_z_last < (-280))//从360度到0度突变
                        {
                            circle_yaw++;
                        }
						
                    }
				imu->data.angle.yaw = a_z - imu->data.angle.first_yaw + circle_yaw*360;
				}
				
				
                if(imu->state.length==47)
                {
                    byte[0] = imu->state.data[9+sizeof(float)*2];
                    byte[1] = imu->state.data[9+sizeof(float)*2+1];
                    byte[2] = imu->state.data[9+sizeof(float)*2+2];
                    byte[3] = imu->state.data[9+sizeof(float)*2+3];	
                    memcpy(&v_z,byte,sizeof(float));
                    imu->data.basedata.gyro[2] = v_z;
					
                    byte[0] = imu->state.data[9+sizeof(float)*1];
                    byte[1] = imu->state.data[9+sizeof(float)*1+1];
                    byte[2] = imu->state.data[9+sizeof(float)*1+2];
                    byte[3] = imu->state.data[9+sizeof(float)*1+3];
                    memcpy(&v_y,byte,sizeof(float));
                    imu->data.basedata.gyro[1] = v_y;
					
					byte[0] = imu->state.data[9+sizeof(float)*0];
                    byte[1] = imu->state.data[9+sizeof(float)*0+1];
                    byte[2] = imu->state.data[9+sizeof(float)*0+2];
                    byte[3] = imu->state.data[9+sizeof(float)*0+3];	
                    memcpy(&v_x,byte,sizeof(float));
                    imu->data.basedata.gyro[0] = v_x;
					mpu_flag = 1;
                }
			GIMBAL_MONITOR.mpu_on++;
			}
            break;

        default: 
        imu->state.packstate = IMU_PACK_STATE_HEADER;
        break; 
		}
 }

 /******************************************************
 * @data	2019.4.28	0:53
 * @brie	串口5DMA接收中断的回调函数
 * @input	rxdata 串口接收的数据
 * @output	无
 * @mader	陈星
 * @note	陀螺仪数据迭代
 * ***************************************************/
void TransducerM_UART_DMA_Rev(P_IMU_DATA pimu_data, uint8_t data[])
{
	static uint8_t DMARecError = 0;
    if ((data[0] == TRANSDUCER_HEAD_1) && (data[1] == TRANSDUCER_HEAD_2))
    {
        uint8_t CRC_check[2] = {0};
		    uint8_t PackDataLength = 0;
        PackDataLength = data[2];
        uint16_t imu1500_check = Checksum_Generate((char *)&data[2], PackDataLength + 1);
        memcpy(CRC_check, &imu1500_check, 2);
        if (CRC_check[0] == data[PackDataLength + 3] && CRC_check[1] == data[PackDataLength + 4])
        {
            if (PackDataLength == 20)
            {
							  pimu_data->angle.timeStamp = data[10]*256*256*256+data[9]*256*256+data[8]*256+data[7];
                a_x= Trans_Char2Float(&data[11]);//row轴从15开始
                pimu_data->angle.pitch = a_x;
							a_y  = Trans_Char2Float(&data[15]); 
						
							                 
							 pimu_data->angle.roll = a_y;
                a_z_last = a_z;
							    
               a_z  = Trans_Char2Float(&data[19]);
                if (pimu_data->angle.first_flag == 0)
                {
                    static uint8_t imu_first_yaw_count = 0;
                    if (a_z != 0)
                    {
                        pimu_data->angle.first_yaw = a_z;
                        if (pimu_data->angle.first_yaw < 10 && pimu_data->angle.first_yaw > -10)
                        {
                            imu_first_yaw_count++; //接受值在-10到10跳变则计数
                        }
                        else
                        {
                            imu_first_yaw_count = 10;
                        }
                        if (imu_first_yaw_count >= 10)
                        {
                            pimu_data->angle.first_flag = 1;
                        }
                        else
                            pimu_data->angle.first_flag = 0;
                    }
                }
                else if (pimu_data->angle.first_flag == 1)
                {
				
                    if (a_z - a_z_last > 280) //从0度到360度突变
                    {
                        pimu_data->angle.circle_yaw--;
                    }
                    else if (a_z - a_z_last < (-280)) //从360度到0度突变
                    {
                        pimu_data->angle.circle_yaw++;
                    }
					
                }
                pimu_data->angle.yaw = a_z - pimu_data->angle.first_yaw + pimu_data->angle.circle_yaw * 360;
            }
            else if (PackDataLength == 44)
            {
				pimu_data->basedata.gyro[0] = Trans_Char2Float(&data[9 + sizeof(float) * 0 + 2]);
                pimu_data->basedata.gyro[1] = Trans_Char2Float(&data[9 + sizeof(float) * 1 + 2]);
                pimu_data->basedata.gyro[2] = Trans_Char2Float(&data[9 + sizeof(float) * 2 + 2]);
            }
        }
				GIMBAL_MONITOR.mpu_on++;
    }
    else
    {
        DMARecError++;
    }
}

 /******************************************************
 * @data	2019.4.28	0:53
 * @brie	将串口发送的4字节数据转化为浮点型数
 * @input	pData 待转化数据
 * @output	无
 * @mader	陈星
 * @note	
 * ***************************************************/
static float Trans_Char2Float(uint8_t *pData)
{
    typedef union
    {
        uint8_t charData[4];
        float floatData;
    } _GYRO_TransData, *P_GYRO_TransData;

    _GYRO_TransData tmpDataBuff = {0};
    for (uint8_t i = 0; i < 4; i++)
    {
        tmpDataBuff.charData[i] = *(pData + i);
    }
    return (tmpDataBuff.floatData);
}
 /******************************************************
 * @data	2019.4.28	0:53
 * @brie	陀螺仪数据校验
 * @input	Data 数据 dataLength 数据长度
 * @output	无
 * @mader	陈星
 * @note	陀螺仪数据校验
 * ***************************************************/
static uint16_t Checksum_Generate(char *data, int dataLength)
{
    uint16_t checkSum = 0;
    unsigned char *d = (unsigned char *)data;
    unsigned char c;
    checkSum = 0xffff;
    for (int i = 0; i < dataLength; i++)
    {
            checkSum ^= (unsigned int)(*(d++));
        for (int j = 0; j < 8; j++)
        {
            c = checkSum & 0x0001;
            checkSum >>= 1;
            if (c)
                checkSum ^= 0xa001;
        }
    }
    return checkSum;
}
 
